// 本文件的目的是：将输入发热bag文件转换成RGB-D图像集
#include <iostream>
#include <string>
#include <cstdint>
#include <algorithm>
#include <map>
#include <atomic>
#include <cstring>
#include <functional>
#include <fstream>

#include <chrono>

#include "librealsense2/rs.hpp"

#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/core.hpp>

const static int OVERRIDE_TIMESTAMP = 0;

//command: ./bag2klg path/to/bag/file outDir_RGB outDir_Depth
int main(int argc,char *argv[]){
	using namespace std::chrono;
	std::string fileName = argv[1];
	std::string outDir_RGB = argv[2];
	std::string outDir_Depth = argv[3];

	// load .bag file
	rs2::config cfg;
	cfg.enable_device_from_file(fileName, 0);
	rs2::pipeline pipe;
	pipe.start(cfg);
	
	//深度信息渲染
	rs2::colorizer colorMap;
	
	//定义和设置深度滤波器
    rs2::spatial_filter spat_filter;
    rs2::threshold_filter thresh_filter;
	spat_filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, 0.5f);
    spat_filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, 20.0f);
	spat_filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, 2.0f);
    spat_filter.set_option(RS2_OPTION_HOLES_FILL, 2.0f);
	thresh_filter.set_option(RS2_OPTION_MAX_DISTANCE, 5.0f);
    thresh_filter.set_option(RS2_OPTION_MIN_DISTANCE,  0.1f);
	
	// get device depth scale
	rs2::device device = pipe.get_active_profile().get_device();
	float depthScale = device.first<rs2::depth_sensor>().get_depth_scale();;
	int64_t timestamp;

	rs2::align align(RS2_STREAM_COLOR);
	rs2::frameset fs;
	int nFrames = 0;
	try {
		while (fs = pipe.wait_for_frames()) {
			
			if (OVERRIDE_TIMESTAMP) {
				// manual timestamp
				timestamp = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count() % 1000000;
				std::cout << "timestamp:" << timestamp << std::endl;
			} else {
				// read timestamp from .bag file
				timestamp = fs.get_color_frame().get_timestamp();
				timestamp = timestamp % 1000000;
			}
			std::cout << "timestamp:" << timestamp << std::endl;

			// align frames
			auto aligned_fs = align.process(fs);
			
			//滤波处理
			aligned_fs  = aligned_fs.apply_filter(spat_filter);
	        aligned_fs  = aligned_fs.apply_filter(thresh_filter);

			auto color_frame = aligned_fs.get_color_frame();
			auto depth_frame = aligned_fs.get_depth_frame();
						
			cv::Mat color(cv::Size(640, 480), CV_8UC3, (void*)color_frame.get_data(), cv::Mat::AUTO_STEP);
		    cv::Mat depth(cv::Size(640, 480),CV_16U,(void*)depth_frame .get_data(),cv::Mat::AUTO_STEP);      //  0  ～  65535

			cv::imwrite(outDir_RGB+"/"+std::to_string(nFrames)+".png", color);
			cv::imwrite(outDir_Depth+"/"+std::to_string(nFrames)+".png",depth);
			nFrames++;
		}
	}
	catch (const rs2::error &e) {
		std::cout << "file ended." << std::endl;
	}

	pipe.stop();
	std::cout << nFrames << " frames." << std::endl;
	std::cout << "saved." << std::endl;

}
